#include "./nozzle_motor.h"
#include "../device/gpio.h"
#include "../device/timer.h"
#include "./mux.h"
#include "../utils/progress.h"

namespace driver {

using namespace device::gpio;
using namespace utils;
using namespace device::timer;

using PULSE = PB<0>; // TIM3_CH3
using DIR = PB<3>;
using ENABLE = PB<5>;
using ALM = PB<4>;
using PWM = PWMTimer<TIM3_BASE, 72 - 1, 3>;

enum class Dir : uint8_t {
    UP = 0,
    DOWN = 1,
    RELEASE = 2,
};

static void set_dir(Dir dir) {
    switch (dir) {
        case Dir::UP: 
            ENABLE::clr();
            DIR::set();
        break;

        case Dir::DOWN:
            ENABLE::clr();
            DIR::clr();
        break;

        case Dir::RELEASE:
            ENABLE::set();
        break;
    }
}

bool NozzleMotor::is_alm() {
    return !ALM::read();
}

static Error puls_once() {
    PULSE::set();
    osDelay(2);
    PULSE::clr();
    osDelay(2);
    if (NozzleMotor::is_alm()) {
        return ec::NOZZLE_LOCKED;
    }
    return ec::OK;
}

NozzleMotor::NozzleMotor() {
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;

    notify();

    config_pp_out<PULSE, DIR, ENABLE>();
    config_float_input<ALM>();
    PWM::stop();
    set_dir(Dir::RELEASE);
    NVIC_EnableIRQ(TIM3_IRQn);
}

Error NozzleMotor::run(uint32_t mm) {
    mm = mm * 400 / 4;
    Error err = async_run(mm);
    if (err) {
        goto finish;
    }
    err = wait_for();
finish:
    return err;
}

Error NozzleMotor::wait_for() {
    Error err;
    await();
    
    if (is_alm()) {
        err = ec::NOZZLE_LOCKED;
        goto finish;
    }

    if (m_position == 0) {
        err = reset_adj();
        goto finish;
    }

finish:
    m_reset_flag = err.is_ok();
    notify();
    return err;
}

Error NozzleMotor::async_run(uint32_t position) {
    Error err;
    await();

    if (is_alm()) {
        err = ec::NOZZLE_LOCKED;
        notify();
        return err;
    }

    if (!m_reset_flag) {
        err = reset();
        if (err) {
            notify();
            return err;
        }
        m_position = 0;
    }

    if (position > m_position) {
        set_dir(Dir::DOWN);
        run_motor(position - m_position);
    } else if (position < m_position) {
        set_dir(Dir::UP);
        run_motor(m_position - position);
    } else {
        notify();
    }
    m_position = position;

    return err;
}

Error NozzleMotor::reset() {
    Error err;
    if (is_alm()) {
        err = ec::NOZZLE_LOCKED;
        goto finish;
    }

    if (!mux->is_nozzle_top()) {
        config_af_pp_out<PULSE>();
        set_dir(Dir::UP);
        PWM::start_with_no_irq(RESET_FREQ);
        Progress prog;
        for (;;) {
            osDelay(20);
            if (mux->is_nozzle_top()) {
                break;
            }
            if (prog.is_after(RESET_TIMEOUT)) {
                err = ec::NOZZLE_TIMEOUT;
                goto finish;
            }
        }
        PWM::stop();
    }
    err = reset_adj();
finish:
    PWM::stop();
    return err;
}

Error NozzleMotor::reset_adj() {
    Error err;
    uint32_t count = 0;
    config_pp_out<PULSE>();
    if (mux->is_nozzle_top()) {
        set_dir(Dir::DOWN);
        for (;;) {
            err = puls_once();
            if (err) {
                goto finish;
            }
            count ++;
            if (count >= ADJ_MAX_COUNT) {
                err = ec::NOZZLE_TIMEOUT;
                goto finish;
            }
            if (!mux->is_nozzle_top()) {
                break;
            }
        }
    }
    set_dir(Dir::UP);
    count = 0;
    for (;;) {
        err = puls_once();
        if (err) {
            goto finish;
        }
        count ++;
        if (count >= ADJ_MAX_COUNT) {
             err = ec::NOZZLE_TIMEOUT;
             goto finish;
         }
        if (mux->is_nozzle_top()) {
            break;
        }
    }
finish:
    m_reset_flag = err.is_ok();
    return err;
}

void NozzleMotor::disable_pwm() {
    PWM::stop();
}

void NozzleMotor::notify() {
    m_flags.set_flags(FLAG_COMPLETE);
}
void NozzleMotor::await() {
    m_flags.wait_any_flags(FLAG_COMPLETE);
}

void NozzleMotor::set_pwm_freq(uint32_t freq) {
    PWM::set_pwm_freq(freq);
}

void NozzleMotor::enable_pwm(uint32_t base_freq) {
    config_af_pp_out<PULSE>();
    PWM::start_with_irq(base_freq);
}

}

using namespace driver;

extern "C"
void TIM3_IRQHandler() {
    if (TIM3->SR & TIM_SR_UIF) {
        TIM3->SR &= ~TIM_SR_UIF;
        nozzleMotor->pwm_epoch_update();
    }
}
